/*
* Copyright (c) 2006-2007 Erin Catto http:
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked, and must not be
* misrepresented the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

goog.provide('box2d.PrismaticJoint');

goog.require('box2d.Jacobian');
goog.require('box2d.Joint');
goog.require('box2d.PrismaticJointDef');
goog.require('box2d.Vec2');

// Linear constraint (point-to-line)
// d = p2 - p1 = x2 + r2 - x1 - r1
// C = dot(ay1, d)
// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
//      = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
//
// Angular constraint
// C = a2 - a1 + a_initial
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// Motor/Limit linear constraint
// C = dot(ax1, d)
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
/**
 @constructor
 @extends {box2d.Joint}
 @param {!box2d.PrismaticJointDef} def
 */
box2d.PrismaticJoint = function(def) {
  // The constructor for b2Joint
  // initialize instance variables for references
  this.m_node1 = new box2d.JointNode();
  this.m_node2 = new box2d.JointNode();
  //
  this.m_type = def.type;
  this.m_prev = null;
  this.m_next = null;
  this.m_body1 = def.body1;
  this.m_body2 = def.body2;
  this.m_collideConnected = def.getCollideConnected();
  this.m_islandFlag = false;
  this.m_userData = def.userData;
  //
  // initialize instance variables for references
  this.m_localAnchor1 = new box2d.Vec2();
  this.m_localAnchor2 = new box2d.Vec2();
  this.m_localXAxis1 = new box2d.Vec2();
  this.m_localYAxis1 = new box2d.Vec2();
  this.m_linearJacobian = new box2d.Jacobian();
  this.m_motorJacobian = new box2d.Jacobian();
  //
  //super(def);
  var tMat;
  var tX;
  var tY;

  //this.m_localAnchor1 = box2d.Math.b2MulTMV(this.m_body1.m_R, box2d.Vec2.subtract(def.anchorPoint , this.m_body1.m_position));
  tMat = this.m_body1.m_R;
  tX = (def.anchorPoint.x - this.m_body1.m_position.x);
  tY = (def.anchorPoint.y - this.m_body1.m_position.y);
  this.m_localAnchor1.Set((tX * tMat.col1.x + tY * tMat.col1.y), (tX * tMat.col2.x + tY * tMat.col2.y));

  //this.m_localAnchor2 = box2d.Math.b2MulTMV(this.m_body2.m_R, box2d.Vec2.subtract(def.anchorPoint , this.m_body2.m_position));
  tMat = this.m_body2.m_R;
  tX = (def.anchorPoint.x - this.m_body2.m_position.x);
  tY = (def.anchorPoint.y - this.m_body2.m_position.y);
  this.m_localAnchor2.Set((tX * tMat.col1.x + tY * tMat.col1.y), (tX * tMat.col2.x + tY * tMat.col2.y));

  //this.m_localXAxis1 = box2d.Math.b2MulTMV(this.m_body1.m_R, def.axis);
  tMat = this.m_body1.m_R;
  tX = def.axis.x;
  tY = def.axis.y;
  this.m_localXAxis1.Set((tX * tMat.col1.x + tY * tMat.col1.y), (tX * tMat.col2.x + tY * tMat.col2.y));

  //this.m_localYAxis1 = box2d.Vec2.crossScalar(1.0, this.m_localXAxis1);
  this.m_localYAxis1.x = -this.m_localXAxis1.y;
  this.m_localYAxis1.y = this.m_localXAxis1.x;

  this.m_initialAngle = this.m_body2.m_rotation - this.m_body1.m_rotation;

  this.m_linearJacobian.SetZero();
  this.m_linearMass = 0.0;
  this.m_linearImpulse = 0.0;

  this.m_angularMass = 0.0;
  this.m_angularImpulse = 0.0;

  this.m_motorJacobian.SetZero();
  this.m_motorMass = 0.0;
  this.m_motorImpulse = 0.0;
  this.m_limitImpulse = 0.0;
  this.m_limitPositionImpulse = 0.0;

  this.m_lowerTranslation = def.lowerTranslation;
  this.m_upperTranslation = def.upperTranslation;
  this.m_maxMotorForce = def.motorForce;
  this.m_motorSpeed = def.motorSpeed;
  this.m_enableLimit = def.enableLimit;
  this.m_enableMotor = def.enableMotor;
};
goog.inherits(box2d.PrismaticJoint, box2d.Joint);

box2d.PrismaticJoint.prototype.GetAnchor1 = function() {
  var b1 = this.m_body1;
  //return box2d.Vec2.add(b1.m_position, box2d.Math.b2MulMV(b1.m_R, this.m_localAnchor1));
  var tVec = new box2d.Vec2();
  tVec.SetV(this.m_localAnchor1);
  tVec.MulM(b1.m_R);
  tVec.add(b1.m_position);
  return tVec;
};
box2d.PrismaticJoint.prototype.GetAnchor2 = function() {
  var b2 = this.m_body2;
  //return box2d.Vec2.add(b2.m_position, box2d.Math.b2MulMV(b2.m_R, this.m_localAnchor2));
  var tVec = new box2d.Vec2();
  tVec.SetV(this.m_localAnchor2);
  tVec.MulM(b2.m_R);
  tVec.add(b2.m_position);
  return tVec;
};
box2d.PrismaticJoint.prototype.GetJointTranslation = function() {
  var b1 = this.m_body1;
  var b2 = this.m_body2;

  var tMat;

  //var r1 = box2d.Math.b2MulMV(b1.m_R, this.m_localAnchor1);
  tMat = b1.m_R;
  var r1X = tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y;
  var r1Y = tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y;
  //var r2 = box2d.Math.b2MulMV(b2.m_R, this.m_localAnchor2);
  tMat = b2.m_R;
  var r2X = tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y;
  var r2Y = tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y;
  //var p1 = box2d.Vec2.add(b1.m_position , r1);
  var p1X = b1.m_position.x + r1X;
  var p1Y = b1.m_position.y + r1Y;
  //var p2 = box2d.Vec2.add(b2.m_position , r2);
  var p2X = b2.m_position.x + r2X;
  var p2Y = b2.m_position.y + r2Y;
  //var d = box2d.Vec2.subtract(p2, p1);
  var dX = p2X - p1X;
  var dY = p2Y - p1Y;
  //var ax1 = box2d.Math.b2MulMV(b1.m_R, this.m_localXAxis1);
  tMat = b1.m_R;
  var ax1X = tMat.col1.x * this.m_localXAxis1.x + tMat.col2.x * this.m_localXAxis1.y;
  var ax1Y = tMat.col1.y * this.m_localXAxis1.x + tMat.col2.y * this.m_localXAxis1.y;

  //var translation = goog.math.Vec2.dot(ax1, d);
  var translation = ax1X * dX + ax1Y * dY;
  return translation;
};
box2d.PrismaticJoint.prototype.GetJointSpeed = function() {
  var b1 = this.m_body1;
  var b2 = this.m_body2;

  var tMat;

  //var r1 = box2d.Math.b2MulMV(b1.m_R, this.m_localAnchor1);
  tMat = b1.m_R;
  var r1X = tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y;
  var r1Y = tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y;
  //var r2 = box2d.Math.b2MulMV(b2.m_R, this.m_localAnchor2);
  tMat = b2.m_R;
  var r2X = tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y;
  var r2Y = tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y;
  //var p1 = box2d.Vec2.add(b1.m_position , r1);
  var p1X = b1.m_position.x + r1X;
  var p1Y = b1.m_position.y + r1Y;
  //var p2 = box2d.Vec2.add(b2.m_position , r2);
  var p2X = b2.m_position.x + r2X;
  var p2Y = b2.m_position.y + r2Y;
  //var d = box2d.Vec2.subtract(p2, p1);
  var dX = p2X - p1X;
  var dY = p2Y - p1Y;
  //var ax1 = box2d.Math.b2MulMV(b1.m_R, this.m_localXAxis1);
  tMat = b1.m_R;
  var ax1X = tMat.col1.x * this.m_localXAxis1.x + tMat.col2.x * this.m_localXAxis1.y;
  var ax1Y = tMat.col1.y * this.m_localXAxis1.x + tMat.col2.y * this.m_localXAxis1.y;

  var v1 = b1.m_linearVelocity;
  var v2 = b2.m_linearVelocity;
  var w1 = b1.m_angularVelocity;
  var w2 = b2.m_angularVelocity;

  //var speed = goog.math.Vec2.dot(d, box2d.Vec2.crossScalar(w1, ax1)) + goog.math.Vec2.dot(ax1, box2d.Vec2.subtract( box2d.Vec2.subtract( box2d.Vec2.add( v2 , box2d.Vec2.crossScalar(w2, r2)) , v1) , box2d.Vec2.crossScalar(w1, r1)));
  //var b2D = (dX*(-w1 * ax1Y) + dY*(w1 * ax1X));
  //var b2D2 = (ax1X * ((( v2.x + (-w2 * r2Y)) - v1.x) - (-w1 * r1Y)) + ax1Y * ((( v2.y + (w2 * r2X)) - v1.y) - (w1 * r1X)));
  var speed = (dX * (-w1 * ax1Y) + dY * (w1 * ax1X)) + (ax1X * (((v2.x + (-w2 * r2Y)) - v1.x) - (-w1 * r1Y)) + ax1Y * (((v2.y + (w2 * r2X)) - v1.y) - (w1 * r1X)));

  return speed;
};
box2d.PrismaticJoint.prototype.GetMotorForce = function(invTimeStep) {
  return invTimeStep * this.m_motorImpulse;
};
box2d.PrismaticJoint.prototype.SetMotorSpeed = function(speed) {
  this.m_motorSpeed = speed;
};
box2d.PrismaticJoint.prototype.SetMotorForce = function(force) {
  this.m_maxMotorForce = force;
};
box2d.PrismaticJoint.prototype.GetReactionForce = function(invTimeStep) {
  var tImp = invTimeStep * this.m_limitImpulse;
  var tMat;

  //var ax1 = box2d.Math.b2MulMV(this.m_body1.m_R, this.m_localXAxis1);
  tMat = this.m_body1.m_R;
  var ax1X = tImp * (tMat.col1.x * this.m_localXAxis1.x + tMat.col2.x * this.m_localXAxis1.y);
  var ax1Y = tImp * (tMat.col1.y * this.m_localXAxis1.x + tMat.col2.y * this.m_localXAxis1.y);
  //var ay1 = box2d.Math.b2MulMV(this.m_body1.m_R, this.m_localYAxis1);
  var ay1X = tImp * (tMat.col1.x * this.m_localYAxis1.x + tMat.col2.x * this.m_localYAxis1.y);
  var ay1Y = tImp * (tMat.col1.y * this.m_localYAxis1.x + tMat.col2.y * this.m_localYAxis1.y);

  //return (invTimeStep * this.m_limitImpulse) * ax1 + (invTimeStep * this.m_linearImpulse) * ay1;
  return new box2d.Vec2(ax1X + ay1X, ax1Y + ay1Y);
};
box2d.PrismaticJoint.prototype.GetReactionTorque = function(invTimeStep) {
  return invTimeStep * this.m_angularImpulse;
};

//--------------- Internals Below -------------------
box2d.PrismaticJoint.prototype.PrepareVelocitySolver = function() {
  var b1 = this.m_body1;
  var b2 = this.m_body2;

  var tMat;

  // Compute the effective masses.
  //box2d.Vec2 r1 = b2Mul(b1->m_R, this.m_localAnchor1);
  tMat = b1.m_R;
  var r1X = tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y;
  var r1Y = tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y;
  //box2d.Vec2 r2 = b2Mul(b2->m_R, this.m_localAnchor2);
  tMat = b2.m_R;
  var r2X = tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y;
  var r2Y = tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y;

  //float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
  var invMass1 = b1.m_invMass;
  var invMass2 = b2.m_invMass;
  //float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
  var invI1 = b1.m_invI;
  var invI2 = b2.m_invI;

  // Compute point to line constraint effective mass.
  // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
  //box2d.Vec2 ay1 = b2Mul(b1->m_R, this.m_localYAxis1);
  tMat = b1.m_R;
  var ay1X = tMat.col1.x * this.m_localYAxis1.x + tMat.col2.x * this.m_localYAxis1.y;
  var ay1Y = tMat.col1.y * this.m_localYAxis1.x + tMat.col2.y * this.m_localYAxis1.y;
  //box2d.Vec2 e = b2->m_position + r2 - b1->m_position;
  var eX = b2.m_position.x + r2X - b1.m_position.x;
  var eY = b2.m_position.y + r2Y - b1.m_position.y;

  //this.m_linearJacobian.Set(-ay1, -box2d.Math.b2Cross(e, ay1), ay1, box2d.Math.b2Cross(r2, ay1));
  this.m_linearJacobian.linear1.x = -ay1X;
  this.m_linearJacobian.linear1.y = -ay1Y;
  this.m_linearJacobian.linear2.x = ay1X;
  this.m_linearJacobian.linear2.y = ay1Y;
  this.m_linearJacobian.angular1 = -(eX * ay1Y - eY * ay1X);
  this.m_linearJacobian.angular2 = r2X * ay1Y - r2Y * ay1X;

  this.m_linearMass = invMass1 + invI1 * this.m_linearJacobian.angular1 * this.m_linearJacobian.angular1 + invMass2 + invI2 * this.m_linearJacobian.angular2 * this.m_linearJacobian.angular2;
  //box2d.Settings.b2Assert(this.m_linearMass > Number.MIN_VALUE);
  this.m_linearMass = 1.0 / this.m_linearMass;

  // Compute angular constraint effective mass.
  this.m_angularMass = 1.0 / (invI1 + invI2);

  // Compute motor and limit terms.
  if (this.m_enableLimit || this.m_enableMotor) {
    // The motor and limit share a Jacobian and effective mass.
    //box2d.Vec2 ax1 = b2Mul(b1->m_R, this.m_localXAxis1);
    tMat = b1.m_R;
    var ax1X = tMat.col1.x * this.m_localXAxis1.x + tMat.col2.x * this.m_localXAxis1.y;
    var ax1Y = tMat.col1.y * this.m_localXAxis1.x + tMat.col2.y * this.m_localXAxis1.y;
    //this.m_motorJacobian.Set(-ax1, -b2Cross(e, ax1), ax1, b2Cross(r2, ax1));
    this.m_motorJacobian.linear1.x = -ax1X;
    this.m_motorJacobian.linear1.y = -ax1Y;
    this.m_motorJacobian.linear2.x = ax1X;
    this.m_motorJacobian.linear2.y = ax1Y;
    this.m_motorJacobian.angular1 = -(eX * ax1Y - eY * ax1X);
    this.m_motorJacobian.angular2 = r2X * ax1Y - r2Y * ax1X;

    this.m_motorMass = invMass1 + invI1 * this.m_motorJacobian.angular1 * this.m_motorJacobian.angular1 + invMass2 + invI2 * this.m_motorJacobian.angular2 * this.m_motorJacobian.angular2;
    //box2d.Settings.b2Assert(this.m_motorMass > Number.MIN_VALUE);
    this.m_motorMass = 1.0 / this.m_motorMass;

    if (this.m_enableLimit) {
      //box2d.Vec2 d = e - r1;
      var dX = eX - r1X;
      var dY = eY - r1Y;
      //float32 jointTranslation = b2Dot(ax1, d);
      var jointTranslation = ax1X * dX + ax1Y * dY;
      if (Math.abs(this.m_upperTranslation - this.m_lowerTranslation) < 2.0 * box2d.Settings.b2_linearSlop) {
        this.m_limitState = box2d.Joint.e_equalLimits;
      } else if (jointTranslation <= this.m_lowerTranslation) {
        if (this.m_limitState != box2d.Joint.e_atLowerLimit) {
          this.m_limitImpulse = 0.0;
        }
        this.m_limitState = box2d.Joint.e_atLowerLimit;
      } else if (jointTranslation >= this.m_upperTranslation) {
        if (this.m_limitState != box2d.Joint.e_atUpperLimit) {
          this.m_limitImpulse = 0.0;
        }
        this.m_limitState = box2d.Joint.e_atUpperLimit;
      } else {
        this.m_limitState = box2d.Joint.e_inactiveLimit;
        this.m_limitImpulse = 0.0;
      }
    }
  }

  if (this.m_enableMotor == false) {
    this.m_motorImpulse = 0.0;
  }

  if (this.m_enableLimit == false) {
    this.m_limitImpulse = 0.0;
  }

  if (box2d.World.s_enableWarmStarting) {
    //box2d.Vec2 P1 = this.m_linearImpulse * this.m_linearJacobian.linear1 + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.linear1;
    var P1X = this.m_linearImpulse * this.m_linearJacobian.linear1.x + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.linear1.x;
    var P1Y = this.m_linearImpulse * this.m_linearJacobian.linear1.y + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.linear1.y;
    //box2d.Vec2 P2 = this.m_linearImpulse * this.m_linearJacobian.linear2 + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.linear2;
    var P2X = this.m_linearImpulse * this.m_linearJacobian.linear2.x + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.linear2.x;
    var P2Y = this.m_linearImpulse * this.m_linearJacobian.linear2.y + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.linear2.y;
    //float32 L1 = this.m_linearImpulse * this.m_linearJacobian.angular1 - this.m_angularImpulse + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.angular1;
    var L1 = this.m_linearImpulse * this.m_linearJacobian.angular1 - this.m_angularImpulse + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.angular1;
    //float32 L2 = this.m_linearImpulse * this.m_linearJacobian.angular2 + this.m_angularImpulse + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.angular2;
    var L2 = this.m_linearImpulse * this.m_linearJacobian.angular2 + this.m_angularImpulse + (this.m_motorImpulse + this.m_limitImpulse) * this.m_motorJacobian.angular2;

    //b1->m_linearVelocity += invMass1 * P1;
    b1.m_linearVelocity.x += invMass1 * P1X;
    b1.m_linearVelocity.y += invMass1 * P1Y;
    //b1->m_angularVelocity += invI1 * L1;
    b1.m_angularVelocity += invI1 * L1;

    //b2->m_linearVelocity += invMass2 * P2;
    b2.m_linearVelocity.x += invMass2 * P2X;
    b2.m_linearVelocity.y += invMass2 * P2Y;
    //b2->m_angularVelocity += invI2 * L2;
    b2.m_angularVelocity += invI2 * L2;
  } else {
    this.m_linearImpulse = 0.0;
    this.m_angularImpulse = 0.0;
    this.m_limitImpulse = 0.0;
    this.m_motorImpulse = 0.0;
  }

  this.m_limitPositionImpulse = 0.0;

};
box2d.PrismaticJoint.prototype.SolveVelocityConstraints = function(step) {
  var b1 = this.m_body1;
  var b2 = this.m_body2;

  var invMass1 = b1.m_invMass;
  var invMass2 = b2.m_invMass;
  var invI1 = b1.m_invI;
  var invI2 = b2.m_invI;

  var oldLimitImpulse;

  // Solve linear constraint.
  var linearCdot = this.m_linearJacobian.Compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);
  var linearImpulse = -this.m_linearMass * linearCdot;
  this.m_linearImpulse += linearImpulse;

  //b1->m_linearVelocity += (invMass1 * linearImpulse) * this.m_linearJacobian.linear1;
  b1.m_linearVelocity.x += (invMass1 * linearImpulse) * this.m_linearJacobian.linear1.x;
  b1.m_linearVelocity.y += (invMass1 * linearImpulse) * this.m_linearJacobian.linear1.y;
  //b1->m_angularVelocity += invI1 * linearImpulse * this.m_linearJacobian.angular1;
  b1.m_angularVelocity += invI1 * linearImpulse * this.m_linearJacobian.angular1;

  //b2->m_linearVelocity += (invMass2 * linearImpulse) * this.m_linearJacobian.linear2;
  b2.m_linearVelocity.x += (invMass2 * linearImpulse) * this.m_linearJacobian.linear2.x;
  b2.m_linearVelocity.y += (invMass2 * linearImpulse) * this.m_linearJacobian.linear2.y;
  //b2.m_angularVelocity += invI2 * linearImpulse * this.m_linearJacobian.angular2;
  b2.m_angularVelocity += invI2 * linearImpulse * this.m_linearJacobian.angular2;

  // Solve angular constraint.
  var angularCdot = b2.m_angularVelocity - b1.m_angularVelocity;
  var angularImpulse = -this.m_angularMass * angularCdot;
  this.m_angularImpulse += angularImpulse;

  b1.m_angularVelocity -= invI1 * angularImpulse;
  b2.m_angularVelocity += invI2 * angularImpulse;

  // Solve linear motor constraint.
  if (this.m_enableMotor && this.m_limitState != box2d.Joint.e_equalLimits) {
    var motorCdot = this.m_motorJacobian.Compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity) - this.m_motorSpeed;
    var motorImpulse = -this.m_motorMass * motorCdot;
    var oldMotorImpulse = this.m_motorImpulse;
    this.m_motorImpulse = box2d.Math.b2Clamp(this.m_motorImpulse + motorImpulse, -step.dt * this.m_maxMotorForce, step.dt * this.m_maxMotorForce);
    motorImpulse = this.m_motorImpulse - oldMotorImpulse;

    //b1.m_linearVelocity += (invMass1 * motorImpulse) * this.m_motorJacobian.linear1;
    b1.m_linearVelocity.x += (invMass1 * motorImpulse) * this.m_motorJacobian.linear1.x;
    b1.m_linearVelocity.y += (invMass1 * motorImpulse) * this.m_motorJacobian.linear1.y;
    //b1.m_angularVelocity += invI1 * motorImpulse * this.m_motorJacobian.angular1;
    b1.m_angularVelocity += invI1 * motorImpulse * this.m_motorJacobian.angular1;

    //b2->m_linearVelocity += (invMass2 * motorImpulse) * this.m_motorJacobian.linear2;
    b2.m_linearVelocity.x += (invMass2 * motorImpulse) * this.m_motorJacobian.linear2.x;
    b2.m_linearVelocity.y += (invMass2 * motorImpulse) * this.m_motorJacobian.linear2.y;
    //b2->m_angularVelocity += invI2 * motorImpulse * this.m_motorJacobian.angular2;
    b2.m_angularVelocity += invI2 * motorImpulse * this.m_motorJacobian.angular2;
  }

  // Solve linear limit constraint.
  if (this.m_enableLimit && this.m_limitState != box2d.Joint.e_inactiveLimit) {
    var limitCdot = this.m_motorJacobian.Compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);
    var limitImpulse = -this.m_motorMass * limitCdot;

    if (this.m_limitState == box2d.Joint.e_equalLimits) {
      this.m_limitImpulse += limitImpulse;
    } else if (this.m_limitState == box2d.Joint.e_atLowerLimit) {
      oldLimitImpulse = this.m_limitImpulse;
      this.m_limitImpulse = Math.max(this.m_limitImpulse + limitImpulse, 0.0);
      limitImpulse = this.m_limitImpulse - oldLimitImpulse;
    } else if (this.m_limitState == box2d.Joint.e_atUpperLimit) {
      oldLimitImpulse = this.m_limitImpulse;
      this.m_limitImpulse = Math.min(this.m_limitImpulse + limitImpulse, 0.0);
      limitImpulse = this.m_limitImpulse - oldLimitImpulse;
    }

    //b1->m_linearVelocity += (invMass1 * limitImpulse) * this.m_motorJacobian.linear1;
    b1.m_linearVelocity.x += (invMass1 * limitImpulse) * this.m_motorJacobian.linear1.x;
    b1.m_linearVelocity.y += (invMass1 * limitImpulse) * this.m_motorJacobian.linear1.y;
    //b1->m_angularVelocity += invI1 * limitImpulse * this.m_motorJacobian.angular1;
    b1.m_angularVelocity += invI1 * limitImpulse * this.m_motorJacobian.angular1;

    //b2->m_linearVelocity += (invMass2 * limitImpulse) * this.m_motorJacobian.linear2;
    b2.m_linearVelocity.x += (invMass2 * limitImpulse) * this.m_motorJacobian.linear2.x;
    b2.m_linearVelocity.y += (invMass2 * limitImpulse) * this.m_motorJacobian.linear2.y;
    //b2->m_angularVelocity += invI2 * limitImpulse * this.m_motorJacobian.angular2;
    b2.m_angularVelocity += invI2 * limitImpulse * this.m_motorJacobian.angular2;
  }
};
box2d.PrismaticJoint.prototype.SolvePositionConstraints = function() {

  var limitC;
  var oldLimitImpulse;

  var b1 = this.m_body1;
  var b2 = this.m_body2;

  var invMass1 = b1.m_invMass;
  var invMass2 = b2.m_invMass;
  var invI1 = b1.m_invI;
  var invI2 = b2.m_invI;

  var tMat;

  //box2d.Vec2 r1 = b2Mul(b1->m_R, this.m_localAnchor1);
  tMat = b1.m_R;
  var r1X = tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y;
  var r1Y = tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y;
  //box2d.Vec2 r2 = b2Mul(b2->m_R, this.m_localAnchor2);
  tMat = b2.m_R;
  var r2X = tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y;
  var r2Y = tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y;
  //box2d.Vec2 p1 = b1->m_position + r1;
  var p1X = b1.m_position.x + r1X;
  var p1Y = b1.m_position.y + r1Y;
  //box2d.Vec2 p2 = b2->m_position + r2;
  var p2X = b2.m_position.x + r2X;
  var p2Y = b2.m_position.y + r2Y;
  //box2d.Vec2 d = p2 - p1;
  var dX = p2X - p1X;
  var dY = p2Y - p1Y;
  //box2d.Vec2 ay1 = b2Mul(b1->m_R, this.m_localYAxis1);
  tMat = b1.m_R;
  var ay1X = tMat.col1.x * this.m_localYAxis1.x + tMat.col2.x * this.m_localYAxis1.y;
  var ay1Y = tMat.col1.y * this.m_localYAxis1.x + tMat.col2.y * this.m_localYAxis1.y;

  // Solve linear (point-to-line) constraint.
  //float32 linearC = b2Dot(ay1, d);
  var linearC = ay1X * dX + ay1Y * dY;
  // Prevent overly large corrections.
  linearC = box2d.Math.b2Clamp(linearC, -box2d.Settings.b2_maxLinearCorrection, box2d.Settings.b2_maxLinearCorrection);
  var linearImpulse = -this.m_linearMass * linearC;

  //b1->m_position += (invMass1 * linearImpulse) * this.m_linearJacobian.linear1;
  b1.m_position.x += (invMass1 * linearImpulse) * this.m_linearJacobian.linear1.x;
  b1.m_position.y += (invMass1 * linearImpulse) * this.m_linearJacobian.linear1.y;
  //b1->m_rotation += invI1 * linearImpulse * this.m_linearJacobian.angular1;
  b1.m_rotation += invI1 * linearImpulse * this.m_linearJacobian.angular1;
  //b1->m_R.Set(b1->m_rotation);
  //b2->m_position += (invMass2 * linearImpulse) * this.m_linearJacobian.linear2;
  b2.m_position.x += (invMass2 * linearImpulse) * this.m_linearJacobian.linear2.x;
  b2.m_position.y += (invMass2 * linearImpulse) * this.m_linearJacobian.linear2.y;
  b2.m_rotation += invI2 * linearImpulse * this.m_linearJacobian.angular2;
  //b2->m_R.Set(b2->m_rotation);
  var positionError = Math.abs(linearC);

  // Solve angular constraint.
  var angularC = b2.m_rotation - b1.m_rotation - this.m_initialAngle;
  // Prevent overly large corrections.
  angularC = box2d.Math.b2Clamp(angularC, -box2d.Settings.b2_maxAngularCorrection, box2d.Settings.b2_maxAngularCorrection);
  var angularImpulse = -this.m_angularMass * angularC;

  b1.m_rotation -= b1.m_invI * angularImpulse;
  b1.m_R.Set(b1.m_rotation);
  b2.m_rotation += b2.m_invI * angularImpulse;
  b2.m_R.Set(b2.m_rotation);

  var angularError = Math.abs(angularC);

  // Solve linear limit constraint.
  if (this.m_enableLimit && this.m_limitState != box2d.Joint.e_inactiveLimit) {

    //box2d.Vec2 r1 = b2Mul(b1->m_R, this.m_localAnchor1);
    tMat = b1.m_R;
    r1X = tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y;
    r1Y = tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y;
    //box2d.Vec2 r2 = b2Mul(b2->m_R, this.m_localAnchor2);
    tMat = b2.m_R;
    r2X = tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y;
    r2Y = tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y;
    //box2d.Vec2 p1 = b1->m_position + r1;
    p1X = b1.m_position.x + r1X;
    p1Y = b1.m_position.y + r1Y;
    //box2d.Vec2 p2 = b2->m_position + r2;
    p2X = b2.m_position.x + r2X;
    p2Y = b2.m_position.y + r2Y;
    //box2d.Vec2 d = p2 - p1;
    dX = p2X - p1X;
    dY = p2Y - p1Y;
    //box2d.Vec2 ax1 = b2Mul(b1->m_R, this.m_localXAxis1);
    tMat = b1.m_R;
    var ax1X = tMat.col1.x * this.m_localXAxis1.x + tMat.col2.x * this.m_localXAxis1.y;
    var ax1Y = tMat.col1.y * this.m_localXAxis1.x + tMat.col2.y * this.m_localXAxis1.y;

    //float32 translation = b2Dot(ax1, d);
    var translation = (ax1X * dX + ax1Y * dY);
    var limitImpulse = 0.0;

    if (this.m_limitState == box2d.Joint.e_equalLimits) {
      // Prevent large angular corrections
      limitC = box2d.Math.b2Clamp(translation, -box2d.Settings.b2_maxLinearCorrection, box2d.Settings.b2_maxLinearCorrection);
      limitImpulse = -this.m_motorMass * limitC;
      positionError = Math.max(positionError, Math.abs(angularC));
    } else if (this.m_limitState == box2d.Joint.e_atLowerLimit) {
      limitC = translation - this.m_lowerTranslation;
      positionError = Math.max(positionError, -limitC);

      // Prevent large linear corrections and allow some slop.
      limitC = box2d.Math.b2Clamp(limitC + box2d.Settings.b2_linearSlop, -box2d.Settings.b2_maxLinearCorrection, 0.0);
      limitImpulse = -this.m_motorMass * limitC;
      oldLimitImpulse = this.m_limitPositionImpulse;
      this.m_limitPositionImpulse = Math.max(this.m_limitPositionImpulse + limitImpulse, 0.0);
      limitImpulse = this.m_limitPositionImpulse - oldLimitImpulse;
    } else if (this.m_limitState == box2d.Joint.e_atUpperLimit) {
      limitC = translation - this.m_upperTranslation;
      positionError = Math.max(positionError, limitC);

      // Prevent large linear corrections and allow some slop.
      limitC = box2d.Math.b2Clamp(limitC - box2d.Settings.b2_linearSlop, 0.0, box2d.Settings.b2_maxLinearCorrection);
      limitImpulse = -this.m_motorMass * limitC;
      oldLimitImpulse = this.m_limitPositionImpulse;
      this.m_limitPositionImpulse = Math.min(this.m_limitPositionImpulse + limitImpulse, 0.0);
      limitImpulse = this.m_limitPositionImpulse - oldLimitImpulse;
    }

    //b1->m_position += (invMass1 * limitImpulse) * this.m_motorJacobian.linear1;
    b1.m_position.x += (invMass1 * limitImpulse) * this.m_motorJacobian.linear1.x;
    b1.m_position.y += (invMass1 * limitImpulse) * this.m_motorJacobian.linear1.y;
    //b1->m_rotation += invI1 * limitImpulse * this.m_motorJacobian.angular1;
    b1.m_rotation += invI1 * limitImpulse * this.m_motorJacobian.angular1;
    b1.m_R.Set(b1.m_rotation);
    //b2->m_position += (invMass2 * limitImpulse) * this.m_motorJacobian.linear2;
    b2.m_position.x += (invMass2 * limitImpulse) * this.m_motorJacobian.linear2.x;
    b2.m_position.y += (invMass2 * limitImpulse) * this.m_motorJacobian.linear2.y;
    //b2->m_rotation += invI2 * limitImpulse * this.m_motorJacobian.angular2;
    b2.m_rotation += invI2 * limitImpulse * this.m_motorJacobian.angular2;
    b2.m_R.Set(b2.m_rotation);
  }

  return positionError <= box2d.Settings.b2_linearSlop && angularError <= box2d.Settings.b2_angularSlop;

};
